import sensor, image, time
from pyb import UART

output_str_red = "[0,0]"
output_str_blue = "[0,0]"
output_str_purple = "[0,0]"
output_str_yellow = "[0,0]"
output_str_black = "[0,0]"

receive_Data = ""
receive_ON = 0
Work_ON = 1
uart_str = ""  # 定义 uart_str 变量

# 颜色阈值定义
black_threshold = (0, 13, -9, 8, -6, 15)
red_threshold = (100, 0, 127, 8, -12, 127)
blue_threshold = (100, 0, -123, 77, -16, 10)
purple_threshold = (0, 65, 30, 72, -128, -20)
yellow_threshold = (26, 81, -33, -6, 20, 127)

# 串口接受程序 包头为0X2A 包尾为0XA2 中间为数据
def UART_Receive():
    global receive_ON
    global uart_str
    global receive_Data
    while uart.any():
        uart_hex = uart.readchar()
        if uart_hex == 0XA2:  # 包尾
            receive_ON = 0
            receive_Data = uart_str
            uart_str = ""
        if receive_ON == 1:
            uart_str += chr(uart_hex)  # 读取串口数据
        if uart_hex == 0X2A:  # 包头
            receive_ON = 1

sensor.reset()
sensor.set_pixformat(sensor.RGB565)
sensor.set_framesize(sensor.QVGA)
sensor.set_windowing((0, 20, 320, 200))
sensor.skip_frames(10)
sensor.set_auto_whitebal(False)

uart = UART(3, 115200)

# 用于存储识别次数的字典
color_counts = {
    'red': 0,
    'black': 0,
    'blue': 0,
    'purple': 0,
    'yellow': 0
}

# 存储最近检测到的颜色
last_detected_colors = []
filter_size = 5  # 定义滤波窗口大小

def find_max(blobs):
    return max(blobs, key=lambda blob: blob.pixels(), default=None)

def detect(max_blob):
    shape = 0
    if max_blob.solidity() > 0.90 or max_blob.density() > 0.84:
        img.draw_rectangle(max_blob.rect(), color=(255, 255, 255))
        shape = 1
    elif max_blob.density() > 0.6:
        img.draw_circle((max_blob.cx(), max_blob.cy(), int((max_blob.w() + max_blob.h()) / 4)))
        shape = 2
    elif max_blob.density() > 0.4:
        img.draw_rectangle(max_blob.rect(), color=(0, 0, 0))
        shape = 3
    return shape

y_threshold = 65  # 设置 y 坐标的阈值

while True:
    img = sensor.snapshot()
    UART_Receive()

    if receive_Data == "Start":
        Work_ON = 1

    if receive_Data == "Stop":
        Work_ON = 0

    if Work_ON == 1:
        # 每次循环清空计数
        for color in color_counts:
            color_counts[color] = 0

        # 进行颜色识别
        detected_color = None

        if img.find_blobs([red_threshold]):
            max_blob_red = find_max(img.find_blobs([red_threshold]))
            if max_blob_red.w() >= 32 and max_blob_red.h() >= 32:
                shape_red = detect(max_blob_red)
                if shape_red and max_blob_red.cy() > y_threshold:  # 如果找到了形状
                        detected_color = 'red'
                        color_counts['red'] += 1


        if img.find_blobs([blue_threshold]):
            max_blob_blue = find_max(img.find_blobs([blue_threshold]))
            if max_blob_blue.w() >= 32 and max_blob_blue.h() >= 32:
                shape_blue = detect(max_blob_blue)
                if shape_blue and max_blob_blue.cy() > y_threshold:  # 如果找到了形状
                    detected_color = 'blue'
                    color_counts['blue'] += 1

        if img.find_blobs([purple_threshold]):
            max_blob_purple = find_max(img.find_blobs([purple_threshold]))
            if max_blob_purple.w() >= 32 and max_blob_purple.h() >= 32:
                shape_purple = detect(max_blob_purple)
                if shape_purple and max_blob_purple.cy() > y_threshold:  # 如果找到了形状
                    detected_color = 'purple'
                    color_counts['purple'] += 1

        # 记录检测到的颜色
        if detected_color:
            last_detected_colors.append(detected_color)
            if len(last_detected_colors) > filter_size:
                last_detected_colors.pop(0)

            # 找到概率最高的颜色
            total_count = sum(color_counts.values())
            if total_count > 0:
                # 统计最近几次检测到的颜色
                filtered_color_counts = {color: last_detected_colors.count(color) for color in color_counts.keys()}
                detected_color_filtered = max(filtered_color_counts, key=filtered_color_counts.get)
                max_count = filtered_color_counts[detected_color_filtered]

                # 计算概率
                probability = max_count / len(last_detected_colors)  # 使用滤波窗口大小进行概率计算

                # 发送数据
                uart.write(bytearray([0X7A]))  # 包头
                uart.write(detected_color_filtered.encode())  # 确保使用编码
                uart.write(bytearray([0XA7]))  # 包尾
                print('发送颜色数据:', detected_color_filtered, '概率:', probability)

    else:
        print('未识别到颜色')
